from lib.PowerLed import PowerLed
from lib.PowerSound import PowerSound
from lib.PowerScreen import PowerScreen
from lib.PowerControl import PowerControl
import time
from lib.Stepper import Stepper
from setting import *

class PowerTestStepper():
    preSpeed =0
    width =1024
    speed = 0
    def __init__(self):
        super().__init__()
        print("PowerTest init")
        self.screen=PowerScreen(POWER_PIN_OLED_I2C_SCL,POWER_PIN_OLED_I2C_SDA,500000,128,64)
        self.sound=PowerSound(POWER_PIN_SOURND_NO_SOURCE)
        self.led = PowerLed(POWER_PIN_LED_RED,POWER_PIN_LED_GREEN,POWER_PIN_LED_BLUE)
        self.control =PowerControl(POWER_PIN_PS1_Z,POWER_PIN_PS1_X,POWER_PIN_PS1_Y)
        self.s1 = Stepper(POWER_PIN_STEPPER_X1_STEP,POWER_PIN_STEPPER_X1_DIR,en_pin=POWER_PIN_STEPPER_X1_ENA,steps_per_rev=800,speed_sps=3200,timer_id=1)
        self.s2 = Stepper(POWER_PIN_STEPPER_Y1_STEP,POWER_PIN_STEPPER_Y1_DIR,en_pin=POWER_PIN_STEPPER_Y1_ENA,steps_per_rev=800,speed_sps=3200,timer_id=2)

    def testLed(self):
        print("1.PowerTest testLed running")
        for num in range(5): 
            self.led.turnOnRedLed()
            time.sleep(0.2)
            self.led.turnOnGreeLed()
            time.sleep(0.2)
            self.led.turnOnBlueLed()
            time.sleep(0.2)

    def testSoundList(self):
        # 乐谱列表
        music = [1, 1, 5, 5, 6, 6, 5, 0,
                 4, 4, 3, 3, 2, 2, 1, 0,
                 5, 5, 4, 4, 3, 3, 2, 0,
                 5, 5, 4, 4, 3, 3, 2, 0,
                 1, 1, 5, 5, 6, 6, 5, 0,
                 4, 4, 3, 3, 2, 2, 1, 0,]
        self.sound.playToneList(music)
   
    def testSound(self,tone):
        self.sound.playTone(tone)
    
    def testScreenInit(self):
        self.screen.initScreen()
            
    def testScreenMsg(self,msg):
        self.screen.showMessage(msg)
    def testScreenRefresh(self):
        self.screen.showMenu(self.screen.menu_curr_List)
        self.screen.screenRefresh()
    def testControl(self):
        print(self.control.getX())
        print(self.control.getY())
        print(self.control.getButtonValue())
        print(self.control.IsButtonPress())
    
    def runStepper(self,speed):
      print("Speed is " , speed)
      if (speed == self.speed):
          print("speed ==")
          return
      if (speed > 0):
         self.speed = speed
         self.s1.speed(speed)
         self.s1.free_run(1)
      elif (speed < 0):
         self.speed = speed
         self.s1.speed(abs(speed))
         self.s1.free_run(-1)
      else:
         self.s1.stop()            

    def calSpeed(self):
      print("Speed is " , self.speed)
      if (self.control.getX()- 2000 > 1000):
         return self.speed + 100
      elif (self.control.getX()- 2000 < -1000):
          return self.speed - 100
      return self.speed
          
          



if __name__ == '__main__':


    test = PowerTestStepper()
    test.screen.oled.rotate(0)
    test.testScreenInit()
    test.testScreenMsg("1.SRC Done")
    test.testLed()
    test.testScreenMsg("2.LED Done")
    # test.testSoundList()
    test.testScreenMsg("3.Sound Done")
 
    
    while(True):
          print("-----------------")
          time.sleep(0.2)
#           print(pin2.value())
#           time.sleep(0.2) 
          print(f'PS1_X:{test.control.getX()},PS1_Y:{test.control.getY()},z:{test.control.getButtonValue()} ')

        # get position in screen
#           psY_percentage = (test.control.getX()- 2000 )  / 2000
#           speed = int(width * psY_percentage)

          test.runStepper(test.calSpeed())
          

          test.screen.oled.fill(0)
          test.screen.oled.text(f'X:{test.control.getX()}', 10, 10)
          test.screen.oled.text(f'Y:{test.control.getY()}', 10, 20)
          test.screen.oled.text(f'S:{test.speed}', 10, 30)
          test.screen.oled.show()
        
  
# https://github.com/redoxcode/micropython-stepper/tree/main  
#  
# s1.speed(800) #use low speed for the calibration
# s1.free_run(-1) #move backwards
# #  
# # s1.stop() #stop as soon as the switch is triggered
# # s1.overwrite_pos(0) #set position as 0 point
# # s1.target(0) #set the target to the same value to avoid unwanted movement
# # s1.speed(200) #return to default speed
# # s1.track_target() #start stepper again
# # 
# # #calibration finished. Do something else below.
# # s1.target_deg(180)
# # time.sleep(5.0)
# while True:
#     print(s1.get_pos())
#     time.sleep(1)
